#include "SpinDetector.h"
#include "Logger.h"
#include <cmath>
// Logger yaw_log("./../yaw.log");

int count_times;
void SpinDetector::reset()
{
    armor_info_window.clear();
    spin_timestamp_list.clear();
    coord_info_window.clear();
    yaw_window.clear();
    is_spin = false;
    fire_flag = true;

    mean_point = cv::Point3f(0, 0, 0);
    last_yaw_diff = 0;
}

bool SpinDetector::judgeSpin(float yaw, uint32_t timestamp)
{
    spin_once_flag = false;
    if (!spin_timestamp_list.empty())
    {
        if (timestamp - spin_timestamp_list[spin_timestamp_list.size() - 1] > param.spin_timeout)
            reset();
    }
    yaw_window.push_back(yaw);

    // 判断是否为跳变帧
    if (yaw_window.size() >= ARMOR_TO_JUDGE_LENGTH)
    {
        if (abs(yaw_window[yaw_window.size() - 1] - yaw_window[yaw_window.size() - 2]) > param.min_yaw_diff)
        {
            spin_once_flag = true;
            spin_timestamp_list.push_back(timestamp);
            if (spin_timestamp_list.size() > param.min_spin_count &&
                (signbit(last_yaw_diff) ==
                 signbit(yaw_window[yaw_window.size() - 1] - yaw_window[yaw_window.size() - 2])))
            {
                is_spin = true;
            }
            last_yaw_diff = yaw_window[yaw_window.size() - 1] - yaw_window[yaw_window.size() - 2];
        }
    }
    while (yaw_window.size() > ARMOR_WINDOW_LENGTH)
    {
        yaw_window.erase(yaw_window.begin());
    }

    // 判断是否可以开火
    if (is_spin)
    {
        double spin_T = 0.0;
        fire_flag = false;
        if (spin_timestamp_list.size() >= 2)
        {
            for (int i = 0; i < spin_timestamp_list.size() - 1; i++)
            {
                spin_T += (spin_timestamp_list[i + 1] - spin_timestamp_list[i]);
            }
            spin_T /= spin_timestamp_list.size();
            cout << "spin T: " << (double)spin_T / 1000.0 << "s" << endl;
            if ((double)(timestamp - spin_timestamp_list[spin_timestamp_list.size() - 1]) / spin_T < 0.3)
            {
                fire_flag = true;
            }
        }
    }

    return is_spin;
}

bool SpinDetector::judgeSpin(ArmorBox armor, bool if_spin)
{
    spin_once_flag = false;
    if (!spin_timestamp_list.empty())
    {
        if (armor.timestamp - spin_timestamp_list[spin_timestamp_list.size() - 1] > param.spin_timeout)
            reset();
    }

    armor_info_window.push_back(armor);
    if (armor_info_window.size() >= ARMOR_TO_JUDGE_LENGTH)
    {
        if (ifLegal() || if_spin)
        {
            spin_once_flag = true;
            spin_timestamp_list.push_back(armor.timestamp);
            is_spin = (spin_timestamp_list.size() > param.min_spin_count);
        }
    }

    while (armor_info_window.size() > ARMOR_WINDOW_LENGTH)
    {
        armor_info_window.erase(armor_info_window.begin()); // TODO  从头开始消除
    }

    return is_spin;
}

bool SpinDetector::ifLegal()
{
    if (armor_info_window.size() < 2)
        return false;
    ArmorBox ct1 = armor_info_window[armor_info_window.size() - 1];
    ArmorBox ct2 = armor_info_window[armor_info_window.size() - 2];
    cv::Point2f coord_delta = ct1.rect.center - ct2.rect.center;

    float y_delta = fabs(coord_delta.y);
    float x_delta = fabs(coord_delta.x);
    float light_height = ct1.box.height;

    return (x_delta / light_height > param.min_x_delta_ratio && x_delta / light_height < param.max_x_delta_ratio &&
            y_delta / light_height > param.min_y_delta_ratio && y_delta / light_height < param.max_y_delta_ratio);
}

bool SpinDetector::solverSpin(cv::Point3f &coord)
{
    if (armor_info_window.size() < ARMOR_WINDOW_LENGTH || !is_spin)
        return false;

    float mean_distance = 0;
    for (int i = 0; i < coord_info_window.size(); i++)
    {
        mean_distance +=
            sqrt(coord_info_window[i].x * coord_info_window[i].x + coord_info_window[i].y + coord_info_window[i].y);
    }
    mean_distance /= (float)coord_info_window.size();

    coord = cv::Point3f(0, 0, 0);
    int count = 0;
    for (int i = 0; i < coord_info_window.size(); i++)
    {
        if (sqrt(coord_info_window[i].x * coord_info_window[i].x + coord_info_window[i].y + coord_info_window[i].y) <
            mean_distance)
            ;
        {
            count++;
            coord += coord_info_window[i];
        }
    }
    coord /= (float)count;
    double spin_T = 0.0;
    for (int i = 0; i < spin_timestamp_list.size() - 1; i++)
    {
        spin_T += (spin_timestamp_list[i + 1] - spin_timestamp_list[i]);
    }
    spin_T /= spin_timestamp_list.size();
    cout << "spin T: " << (double)spin_T / 1000.0 << "s" << endl;

    return true;
}

void SpinDetector::pushCoord(cv::Point3f coord)
{
    coord_info_window.push_back(coord);
    if (coord_info_window.size() > ARMOR_WINDOW_LENGTH)
    {
        coord_info_window.erase(coord_info_window.begin());
    }
}

void SpinDetector::push_pnp_angle_distance(cv::Point3f pnp_angle_diastance, ArmorState state)
{
    if (last_state != 0 && state == 0)
    {
        // yaw_log.log("Lost");

        if (pnp_angles_vector.size() > 10 && data_cal_vector.size() > 8)
        {
            if (pnp_angle_diastance.y - pnp_angles_vector[pnp_angles_vector.size() - 10].y > 4)
            {
                spindrict = 1;
            }
            else if (pnp_angle_diastance.y - pnp_angles_vector[pnp_angles_vector.size() - 10].y < -4)
            {
                spindrict = 0;
            }
            calculate_angles(data_cal_vector, spindrict);
            // yaw_log.log("bounce angle :"+std::to_string(bounce_angle));
        }
        data_cal_vector.clear();
        pnp_angles_vector.clear();
        last_state = state;
        return;
    }
    else if (last_state == 0 && state == 0)
    {
        if (count_times <= 3)
        {
            bounce_angle = bounce_angle;
            count_times++;
        }
        else
        {
            // bounce_angle=0;
            count_times = 0;
        }
        return;
    }
    if (state == 1)
    {

        // yaw_log.log("First");
        last_state = state;
    }
    pnp_angles_vector.push_back(pnp_angle_diastance);
    if (pnp_angles_vector.size() > 10)
    {
        if (abs(pnp_angle_diastance.y - pnp_angles_vector[pnp_angles_vector.size() - 5].y) > 0.2)
        {
            spin_counter++;
            data_cal_vector.push_back(pnp_angle_diastance);
            // yaw_log.log("real_yaw:"+std::to_string(pnp_angle_diastance.y)+"
            // pnp_yaw:"+std::to_string(pnp_angle_diastance.x));
        }
        else
        {
            spin_counter = 0;
        }

        if (pnp_angle_diastance.y - pnp_angles_vector[pnp_angles_vector.size() - 10].y > 0.05)
        {
            spindrict = 1;
        }
        else if (pnp_angle_diastance.y - pnp_angles_vector[pnp_angles_vector.size() - 10].y < -0.05)
        {
            spindrict = 0;
        }
        else
        {
            spindrict = 2;
        }

        if (spin_counter > 4)
        {
            wether_spin = 1;
        }
        else
        {
            wether_spin = 0;
        }
    }
    // if (pnp_angles_vector.size() > ARMOR_WINDOW_LENGTH)  //这里可能有问题  先不管
    // {
    //     pnp_angles_vector.erase(pnp_angles_vector.begin());
    // }
    // cout << "pnp_angles_vector size: " << pnp_angles_vector.size() << endl;
    if (pnp_angles_vector.size() > 20)
    {
        vector<float> fitting_vector;
        for (int i = 0; i < 10; i++)
        {
            fitting_vector.push_back(pnp_angles_vector[pnp_angles_vector.size() - i].y);
        }
        // detectTrend(fitting_vector);
    }
}

void SpinDetector::detectTrend(const std::vector<float> &fitting_vector)
{
    if (fitting_vector.size() < 2)
    {
        std::cout << "Not enough data for regression" << std::endl;
        return;
    }

    // Step 1: Prepare data for fitting (convert fitting_vector into a 2D matrix)
    std::vector<cv::Point2f> points;
    for (int i = 0; i < fitting_vector.size(); i++)
    {
        points.push_back(cv::Point2f(i, fitting_vector[i])); // x = index, y = fitting_vector[i]
    }

    // Step 2: Fit a line to the points using OpenCV's fitLine
    cv::Vec4f line;
    cv::fitLine(points, line, cv::DIST_L2, 0, 0.01, 0.01);

    // Step 3: Extract the slope (m) from the fitted line
    float slope = line[1]; // The slope is stored in line[1]

    // Step 4: Determine the trend
    if (slope > 0.2)
    {
        std::cout << "The trend is increasing." << std::endl;
        wether_spin = 1;
    }
    else if (slope < -0.2)
    {
        std::cout << "The trend is decreasing." << std::endl;
        wether_spin = 1;
    }
    else
    {
        std::cout << "The trend is stable (no change)." << std::endl;
        wether_spin = 0;
    }

    // Optionally print the fitted line's parameters (for debugging purposes)
    std::cout << "Fitted line parameters: slope = " << slope << ", intercept = " << line[3] << std::endl;
}

SpinDetector::SpinDetector()
{
    reset();
}

void SpinDetector::calculate_angles(vector<cv::Point3f> data_cal_vector, int dirction)
{
    vector<float> yaw_vector;
    int sum = 0;
    int r = 300; // 500mm
    float d1 = data_cal_vector[2].z * 1000;
    float d2 = data_cal_vector[data_cal_vector.size() - 1].z * 1000;
    float yaw1 = d1 = (data_cal_vector[2].x + data_cal_vector[3].x + data_cal_vector[4].x + data_cal_vector[5].x) / 4.0;
    float yaw2 = d1 = (data_cal_vector[data_cal_vector.size() - 1].x + data_cal_vector[data_cal_vector.size() - 2].x +
                       data_cal_vector[data_cal_vector.size() - 3].x + data_cal_vector[data_cal_vector.size() - 4].x) /
                      4.0;
    float temp = r * std::sin((abs(yaw1 - yaw2) / 2.0));
    bounce_angle = abs(std::asin(temp / (d1 + d2) / 2.0) * 8);

    if (yaw1 > yaw2)
    {
        bounce_angle = -bounce_angle * 8;
    }
    else if (yaw1 < yaw2)
    {
        bounce_angle = bounce_angle * 4;
    }
    if (abs(bounce_angle) > 10)
    {
        bounce_angle = 0;
    }

    for (int i = 0; i < data_cal_vector.size() - 1; i++)
    {
        if (data_cal_vector[i + 1].y * data_cal_vector[i].y < 0)
        {
            if (abs(data_cal_vector[i + 1].y - data_cal_vector[i].y) > 20)
            {
                /* code */
            }
        }
        yaw_vector.push_back(data_cal_vector[i].y);
        sum += data_cal_vector[i].y;
    }
    sum /= data_cal_vector.size();
    for (int i = 0; i < data_cal_vector.size() - 1; i++)
    {
        if (abs(data_cal_vector[i].y - sum) > 20)
        {
        }
        yaw_vector.push_back(data_cal_vector[i].y);
        sum += data_cal_vector[i].y;
    }
}